#include <kuai/misc/geometry.hpp>
#include <kuai/misc/constant.hpp>


namespace kuai
{
	void calc1stDerivationOfAngle(const XYZ& v1, const XYZ& v2, XYZ result[])
	{ 
		//           d                  -1 
		// dangle = --- arccos(v) = -------------
		//           dv               sqrt(1-v*v)
		//
		//          v1^v2           x1*x2+y1*y2+z1*z2            
		// v = ----------------- = -------------------
        //      abs(v1)*abs(v2)      abs(v1)*abs(v2)

		// ^ is dot product. v1 ^ v2 = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
		// & is direct product. v1 & v2 = (v1.x*v2.x, v1.y*v2.y, v1.z*v2.z);

		assert (abs(v1) > 0 && abs(v2) > 0);

		RealNumber r1 = abs(v1), r2 = abs(v2);
		XYZ n1 = v1 / r1, n2 = v2 / r2;	// Normalized vector
		RealNumber	n12 = dot(n1, n2);

		result[0].x = (n2.x - n12 * n1.x) / r1;
		result[0].y = (n2.y - n12 * n1.y) / r1;
		result[0].z = (n2.z - n12 * n1.z) / r1;
		result[1].x = (n1.x - n12 * n2.x) / r2;
		result[1].y = (n1.y - n12 * n2.y) / r2;
		result[1].z = (n1.z - n12 * n2.z) / r2;

		RealNumber factor = 1-n12*n12;
		factor = -1/sqrt(factor);
		
		result[0] *= factor;
		result[1] *= factor;
	}

	void calc1stDerivationOfAngle(const XYZ& v1, const XYZ& v2, const XYZ& v3, XYZ result[])
	{ 
		calc1stDerivationOfAngle(v1-v2, v3-v2, result);
		result[2] = result[1];
		result[1] = -result[1] - result[0];
	}

	RealNumber calcDehedralAngle(const XYZ& v1, const XYZ& v2, const XYZ& axis)
	{ 
		XYZ vt = v1 * axis;
		XYZ vu = v2 * axis;

        RealNumber result = angle(vt, vu);

		if (dot(v1, vu) < 0) { 
			result = -result;
		}
		return result;
	}
	RealNumber calcTorsion(const XYZ& v1, const XYZ& v2, const XYZ& axis)
	{ 
		return calcDehedralAngle(v1,v2, axis);
	}
	RealNumber calcCosTorsion(const XYZ& v1, const XYZ& v2, const XYZ& axis)
	{
		XYZ vt = v1 * axis;
		XYZ vu = v2 * axis;

		return calcCosAngle(vt, vu);
	}
	RealNumber calcCosTorsion(const XYZ& v1, const XYZ& v2, const XYZ& axis, XYZ result[])
	{
		XYZ vt = v1 * axis;
		XYZ vu = v2 * axis;

		XYZ temp[2];
		RealNumber cosPhi = calcCosAngle(vt, vu, temp);
		result[0].x = temp[0].z * axis.y - temp[0].y * axis.z;
		result[0].y = temp[0].x * axis.z - temp[0].z * axis.x;
		result[0].z = temp[0].y * axis.x - temp[0].x * axis.y;
		result[1].x = temp[1].z * axis.y - temp[1].y * axis.z;
		result[1].y = temp[1].x * axis.z - temp[1].z * axis.x;
		result[1].z = temp[1].y * axis.x - temp[1].x * axis.y;
		result[2].x = temp[0].y * v1.z - temp[0].z * v1.y + temp[1].y * v2.z - temp[1].z * v2.y;
		result[2].y = temp[0].z * v1.x - temp[0].x * v1.z + temp[1].z * v2.x - temp[1].x * v2.z;
		result[2].z = temp[0].x * v1.y - temp[0].y * v1.x + temp[1].x * v2.y - temp[1].y * v2.x;

		return cosPhi;
	}
	RealNumber calc1stDerivationOfDehedralAngle(const XYZ& v1, const XYZ& v2, const XYZ& axis, XYZ result[])
	{ 
		XYZ vt = v1 * axis;
		XYZ vu = v2 * axis;

		XYZ temp[2];
		calc1stDerivationOfAngle(vt, vu, temp);
		result[0].x = temp[0].z * axis.y - temp[0].y * axis.z;
		result[0].y = temp[0].x * axis.z - temp[0].z * axis.x;
		result[0].z = temp[0].y * axis.x - temp[0].x * axis.y;
		result[1].x = temp[1].z * axis.y - temp[1].y * axis.z;
		result[1].y = temp[1].x * axis.z - temp[1].z * axis.x;
		result[1].z = temp[1].y * axis.x - temp[1].x * axis.y;
		result[2].x = temp[0].y * v1.z - temp[0].z * v1.y + temp[1].y * v2.z - temp[1].z * v2.y;
		result[2].y = temp[0].z * v1.x - temp[0].x * v1.z + temp[1].z * v2.x - temp[1].x * v2.z;
		result[2].z = temp[0].x * v1.y - temp[0].y * v1.x + temp[1].x * v2.y - temp[1].y * v2.x;

		RealNumber phi = angle(vt, vu);
		if (dot(v1, vu) < 0)
		{ 
			phi = -phi;
			result[0] = -result[0];
			result[1] = -result[1];
			result[2] = -result[2];
		}
		return phi;
	}

	RealNumber calcOffPlane(const XYZ& v1, const XYZ& v2, const XYZ& v3, const XYZ& v4)
	{
		XYZ coords[3] = {v2-v1, v3-v1, v4-v1};
		RealNumber theta1 = PI/2 - angle(coords[0], coords[1]*coords[2]);
		RealNumber theta2 = PI/2 - angle(coords[1], coords[2]*coords[0]);
		RealNumber theta3 = PI/2 - angle(coords[2], coords[0]*coords[1]);

		return (theta1 + theta2 + theta3) / 3;
	}

	void calc1stDerivationOfOffPlane(const XYZ& v1, const XYZ& v2, const XYZ& v3, XYZ result[3])
	{ 
		XYZ temp[2];
		calc1stDerivationOfAngle(v1, v2*v3, temp);
		result[0] = -temp[0];

		result[1].x = temp[1].y * v3.z - temp[1].z * v3.y;
		result[1].y = temp[1].z * v3.x - temp[1].x * v3.z;
		result[1].z = temp[1].x * v3.y - temp[1].y * v3.x;
		result[2].x = temp[1].y * v2.z - temp[1].z * v2.y;
		result[2].y = temp[1].z * v2.x - temp[1].x * v2.z;
		result[2].z = temp[1].x * v2.y - temp[1].y * v2.x;
	}

	void calc1stDerivationOfOffPlane(const XYZ& v1, const XYZ& v2, const XYZ& v3, const XYZ& v4, XYZ result[4])
	{ 
		XYZ coords[3] = {v2-v1, v3-v1, v4-v1};
		result[0] = result[1] = result[2] = result[3] = XYZ(0, 0, 0);

		XYZ temp[3];
		calc1stDerivationOfOffPlane(coords[0], coords[1], coords[2], temp);
		result[0] -= temp[0] + temp[1] + temp[2];
		result[1] += temp[0];
		result[2] += temp[1];
		result[3] += temp[2];
		calc1stDerivationOfOffPlane(coords[1], coords[2], coords[0], temp);
		result[0] -= temp[0] + temp[1] + temp[2];
		result[1] += temp[1];
		result[2] += temp[2];
		result[3] += temp[0];
		calc1stDerivationOfOffPlane(coords[2], coords[0], coords[1], temp);
		result[0] -= temp[0] + temp[1] + temp[2];
		result[1] += temp[2];
		result[2] += temp[0];
		result[3] += temp[1];

		result[0] /= 3.0;
		result[1] /= 3.0;
		result[2] /= 3.0;
		result[3] /= 3.0;
	}

	const HessianItem diag(RealNumber v11, RealNumber v22, RealNumber v33)
	{ 
		HessianItem result(0.0);
		result(0, 0) = v11;
		result(1, 1) = v22;
		result(2, 2) = v33;
		return result;
	};

	const HessianItem prod(const XYZ& v1, const XYZ& v2)
	{ 
		HessianItem result;
		result(0, 0) = v1.x * v2.x;
		result(0, 1) = v1.x * v2.y;
		result(0, 2) = v1.x * v2.z;
		result(1, 0) = v1.y * v2.x;
		result(1, 1) = v1.y * v2.y;
		result(1, 2) = v1.y * v2.z;
		result(2, 0) = v1.z * v2.x;
		result(2, 1) = v1.z * v2.y;
		result(2, 2) = v1.z * v2.z;
		return result;
	}
	const HessianItem prod(const XYZ& v1) {
		HessianItem result;
		result(0, 0) = v1.x * v1.x;
		result(0, 1) = result(1, 0) = v1.x * v1.y;
		result(0, 2) = result(2, 0) = v1.x * v1.z;
		result(1, 1) = v1.y * v1.y;
		result(1, 2) = result(2, 1) = v1.y * v1.z;
		result(2, 2) = v1.z * v1.z;
		return result;
	}

	RealNumber calcAtomPair(const XYZ& v1)
	{
		return abs(v1);
	}
	RealNumber calcAtomPair(const XYZ& v1, const XYZ& v2)
	{
		return calcAtomPair(v1-v2);
	}
	RealNumber calcAtomPair(const XYZ& v, XYZ dx[1])
	{ 
		RealNumber r = abs(v);
		dx[0] = v/r;
		return r;
	}
	RealNumber calcAtomPair(const XYZ& v1, const XYZ& v2, XYZ dx[2])
	{
		RealNumber r = calcAtomPair(v1-v2, dx);
		dx[1] = -dx[0];
		return r;
	};
	RealNumber calcAtomPair(const XYZ& v1, XYZ dx[1], HessianItem dx2[1])
	{
		RealNumber r = abs(v1);
		dx[0] = v1 / r;
		dx2[0] = diag(1, 1, 1) - prod(dx[0], dx[0]);
		dx2[0] /= r;
		return r;
	}
	RealNumber calcAtomPair(const XYZ& v1, const XYZ& v2, XYZ dx[2], HessianItem dx2[4])
	{ 
		RealNumber r = calcAtomPair(v1-v2, dx, dx2);
		dx[1] = -dx[0];
		dx2[1] = dx2[2] = -dx2[0];
		dx2[3] = dx2[0];
		return r;
	};

	RealNumber calcCosAngle(const XYZ& v1, const XYZ& v2)
	{
		RealNumber result = 1/sqrt(dot(v1, v1)*dot(v2,v2));
		result *= dot(v1, v2);
		return result;
	};
	RealNumber calcAngle(const XYZ& v1, const XYZ& v2)
	{
		return acos( calcCosAngle(v1, v2) );
	};
	RealNumber calcCosAngle(const XYZ& v1, const XYZ& v2, XYZ dx[2])
	{
		RealNumber rr1 = 1/abs(v1), rr2 = 1/abs(v2);
		XYZ n1 = v1 * rr1, n2 = v2 * rr2;	// Normalized vector
		RealNumber costheta = dot(n1, n2);
		dx[0] = (n2 - costheta * n1) * rr1;
		dx[1] = (n1 - costheta * n2) * rr2;
		return costheta;
	};
	RealNumber calcCosAngle(const XYZ& v1, const XYZ& v2, XYZ dx[2], HessianItem dx2[4])
	{
		RealNumber rr1 = 1/abs(v1), rr2 = 1/abs(v2);
		XYZ n1 = v1 * rr1, n2 = v2 * rr2;	// Normalized vector
		RealNumber cosA = dot(n1, n2);
		dx[0] = (n2 - cosA * n1) * rr1;
		dx[1] = (n1 - cosA * n2) * rr2;

		RealNumber rr1_2 = rr1*rr1, rr2_2 = rr2*rr2, rr12 = rr1*rr2;

		dx2[0][0][0] = (-2*n1.x*n2.x - cosA*(1 - 3*n1.x*n1.x))*rr1_2;
		dx2[0][0][1] = dx2[0][1][0] = (-n1.x*n2.y - n2.x*n1.y + 3*cosA*n1.x*n1.y)*rr1_2;
		dx2[0][0][2] = dx2[0][2][0] = (-n1.x*n2.z - n2.x*n1.z + 3*cosA*n1.x*n1.z)*rr1_2;
		dx2[0][1][1] = (-2*n1.y*n2.y - cosA*(1 - 3*n1.y*n1.y))*rr1_2;
		dx2[0][1][2] = dx2[0][2][1] = (-n1.y*n2.z - n2.y*n1.z + 3*cosA*n1.y*n1.z)*rr1_2;
		dx2[0][2][2] = (-2*n1.z*n2.z - cosA*(1 - 3*n1.z*n1.z))*rr1_2;

		dx2[1][0][0] = dx2[2][0][0] =  (1 + cosA*n1.x*n2.x - n2.x*n2.x - n1.x*n1.x)*rr12;
		dx2[1][0][1] = dx2[2][1][0] =  (-n1.x*n1.y - n2.x*n2.y + cosA*n1.x*n2.y)*rr12;
		dx2[1][0][2] = dx2[2][2][0] =  (-n1.x*n1.z - n2.x*n2.z + cosA*n1.x*n2.z)*rr12;
		dx2[1][1][0] = dx2[2][0][1] =  (-n1.x*n1.y - n2.x*n2.y + cosA*n2.x*n1.y)*rr12;
		dx2[1][1][1] = dx2[2][1][1] =  (1 + cosA*n1.y*n2.y - n2.y*n2.y - n1.y*n1.y)*rr12;
		dx2[1][1][2] = dx2[2][2][1] =  (-n1.y*n1.z - n2.y*n2.z + cosA*n1.y*n2.z)*rr12;
		dx2[1][2][0] = dx2[2][0][2] =  (-n1.x*n1.z - n2.x*n2.z + cosA*n2.x*n1.z)*rr12;
		dx2[1][2][1] = dx2[2][1][2] =  (-n1.y*n1.z - n2.y*n2.z + cosA*n2.y*n1.z)*rr12;
		dx2[1][2][2] = dx2[2][2][2] =  (1 + cosA*n1.z*n2.z - n2.z*n2.z - n1.z*n1.z)*rr12;

		dx2[3][0][0] = (-2*n1.x*n2.x - cosA*(1 - 3*n2.x*n2.x))*rr2_2;
		dx2[3][0][1] = dx2[3][1][0] = (3*cosA*n2.x*n2.y - n1.x*n2.y - n2.x*n1.y)*rr2_2;
		dx2[3][0][2] = dx2[3][2][0] = (3*cosA*n2.x*n2.z - n1.x*n2.z - n2.x*n1.z)*rr2_2;
		dx2[3][1][1] = (-2*n1.y*n2.y - cosA*(1 - 3*n2.y*n2.y))*rr2_2;
		dx2[3][1][2] = dx2[3][2][1] = (3*cosA*n2.y*n2.z - n1.y*n2.z - n2.y*n1.z)*rr2_2;
		dx2[3][2][2] = (-2*n1.z*n2.z - cosA*(1 - 3*n2.z*n2.z))*rr2_2;

		return cosA;
	};
	RealNumber calcAngle(const XYZ& v1, const XYZ& v2, XYZ dx[2])
	{
		RealNumber costheta = calcCosAngle(v1,v2,dx);
		RealNumber factor = -1/sqrt(1- costheta*costheta);
		dx[0] *= factor;
		dx[1] *= factor;
		return acos(costheta);
	};
	
	RealNumber calcCosAngle(const XYZ& v1, const XYZ& v2, const XYZ& v3)
	{
		return calcCosAngle(v1-v2, v3-v2);
	};
	RealNumber calcAngle(const XYZ& v1, const XYZ& v2, const XYZ& v3)
	{
		return angle(v1-v2, v3-v2);
	};
	RealNumber calcCosAngle(const XYZ& v1, const XYZ& v2, const XYZ& v3, XYZ dx[3]) {
		RealNumber cosAngle =calcCosAngle(v1-v2, v3-v2, dx);
		dx[2] = dx[1];
		dx[1] = -dx[1]-dx[0];
		return cosAngle;
	}
	RealNumber calcAngle(const XYZ& v1, const XYZ& v2, const XYZ& v3, XYZ dx[3])
	{
		RealNumber theta = calcAngle(v1-v2, v3-v2, dx);
		dx[2] = dx[1];
		dx[1] = -dx[1] - dx[0];
		return theta;
	};

	RealNumber calcAngle(const XYZ& v1, const XYZ& v2, XYZ dx[2], HessianItem dx2[4])
	{
		RealNumber rr1 = 1/abs(v1), rr2 = 1/abs(v2);
		XYZ n1(v1 * rr1), n2(v2 * rr2);	// Normalized vector
		RealNumber cosA = dot(n1, n2);
		RealNumber theta = acos(cosA);

		RealNumber cosA_2 = cosA * cosA;
		RealNumber rsin2A = 1 / (1-cosA_2);
		RealNumber rsinA = sqrt(rsin2A);

		RealNumber under;
		under = rsinA*rr1;
		dx[0].x = (-n2.x + cosA*n1.x)*under;
		dx[0].y = (-n2.y + cosA*n1.y)*under;
		dx[0].z = (-n2.z + cosA*n1.z)*under;
		under = rsinA*rr2;
		dx[1].x = (-n1.x + cosA*n2.x)*under;
		dx[1].y = (-n1.y + cosA*n2.y)*under;
		dx[1].z = (-n1.z + cosA*n2.z)*under;

		RealNumber nx1_2 = n1.x*n1.x, ny1_2 = n1.y*n1.y, nz1_2 = n1.z*n1.z;
		RealNumber nx2_2 = n2.x*n2.x, ny2_2 = n2.y*n2.y, nz2_2 = n2.z*n2.z;
		
		under = rsinA*rr1*rr1;
		dx2[0][0][0] = ((cosA - 3*cosA*nx1_2 + 2*n1.x*n2.x) - (n2.x - cosA*n1.x)*(cosA*n2.x - n1.x*cosA_2)*rsin2A)*under;
		dx2[0][0][1] = dx2[0][1][0] = ((n1.x*n2.y + n2.x*n1.y - 3*cosA*n1.x*n1.y) - (n2.x - cosA*n1.x)*(cosA*n2.y - n1.y*cosA_2)*rsin2A)*under;
		dx2[0][0][2] = dx2[0][2][0] = ((n1.x*n2.z + n2.x*n1.z - 3*cosA*n1.x*n1.z) - (n2.x - cosA*n1.x)*(cosA*n2.z - n1.z*cosA_2)*rsin2A)*under;
		dx2[0][1][1] = ((cosA - 3*cosA*ny1_2 + 2*n1.y*n2.y) - (n2.y - cosA*n1.y)*(cosA*n2.y - n1.y*cosA_2)*rsin2A)*under;
		dx2[0][1][2] = dx2[0][2][1] = ((n1.y*n2.z + n2.y*n1.z - 3*cosA*n1.y*n1.z) - (n2.y - cosA*n1.y)*(cosA*n2.z - n1.z*cosA_2)*rsin2A)*under;
		dx2[0][2][2] = ((cosA - 3*cosA*nz1_2 + 2*n1.z*n2.z) - (n2.z - cosA*n1.z)*(cosA*n2.z - n1.z*cosA_2)*rsin2A)*under;

		under = rsinA*rr1*rr2;
		dx2[1][0][0] = dx2[2][0][0] = ((-1 + nx1_2 + nx2_2 - cosA*n1.x*n2.x) - (n2.x - cosA*n1.x)*(cosA*n1.x - n2.x*cosA_2)*rsin2A)*under;
		dx2[1][0][1] = dx2[2][1][0] = ((n1.x*n1.y + n2.x*n2.y - cosA*n1.x*n2.y) - (n2.x - cosA*n1.x)*(cosA*n1.y - n2.y*cosA_2)*rsin2A)*under;
		dx2[1][0][2] = dx2[2][2][0] = ((n1.x*n1.z + n2.x*n2.z - cosA*n1.x*n2.z) - (n2.x - cosA*n1.x)*(cosA*n1.z - n2.z*cosA_2)*rsin2A)*under;
		dx2[1][1][0] = dx2[2][0][1] = ((n1.x*n1.y + n2.x*n2.y - cosA*n2.x*n1.y) - (n2.y - cosA*n1.y)*(cosA*n1.x - n2.x*cosA_2)*rsin2A)*under;
		dx2[1][1][1] = dx2[2][1][1] = ((-1 + ny1_2 + ny2_2 - cosA*n1.y*n2.y) - (n2.y - cosA*n1.y)*(cosA*n1.y - n2.y*cosA_2)*rsin2A)*under;
		dx2[1][1][2] = dx2[2][2][1] = ((n1.y*n1.z + n2.y*n2.z - cosA*n1.y*n2.z) - (n2.y - cosA*n1.y)*(cosA*n1.z - n2.z*cosA_2)*rsin2A)*under;
		dx2[1][2][0] = dx2[2][0][2] = ((n1.x*n1.z + n2.x*n2.z - cosA*n2.x*n1.z) - (n2.z - cosA*n1.z)*(cosA*n1.x - n2.x*cosA_2)*rsin2A)*under;
		dx2[1][2][1] = dx2[2][1][2] = ((n1.y*n1.z + n2.y*n2.z - cosA*n2.y*n1.z) - (n2.z - cosA*n1.z)*(cosA*n1.y - n2.y*cosA_2)*rsin2A)*under;
		dx2[1][2][2] = dx2[2][2][2] = ((-1 + nz1_2 + nz2_2 - cosA*n1.z*n2.z) - (n2.z - cosA*n1.z)*(cosA*n1.z - n2.z*cosA_2)*rsin2A)*under;

		under = rsinA*rr2*rr2;
		dx2[3][0][0] = ((cosA - 3*cosA*nx2_2 + 2*n1.x*n2.x) - (cosA*n1.x - n2.x*cosA_2)*(n1.x - cosA*n2.x)*rsin2A)*under;
		dx2[3][0][1] = dx2[3][1][0] = ((n1.x*n2.y + n2.x*n1.y - 3*cosA*n2.x*n2.y) - (n1.x - cosA*n2.x)*(cosA*n1.y - n2.y*cosA_2)*rsin2A)*under;
		dx2[3][0][2] = dx2[3][2][0] = ((n1.x*n2.z + n2.x*n1.z - 3*cosA*n2.x*n2.z) - (n1.x - cosA*n2.x)*(cosA*n1.z - n2.z*cosA_2)*rsin2A)*under;
		dx2[3][1][1] = ((cosA - 3*cosA*ny2_2 + 2*n1.y*n2.y) - (n1.y - cosA*n2.y)*(cosA*n1.y - n2.y*cosA_2)*rsin2A)*under;
		dx2[3][1][2] = dx2[3][2][1] = ((n1.y*n2.z + n2.y*n1.z - 3*cosA*n2.y*n2.z) - (n1.y - cosA*n2.y)*(cosA*n1.z - n2.z*cosA_2)*rsin2A)*under;
		dx2[3][2][2] = ((cosA - 3*cosA*nz2_2 + 2*n1.z*n2.z) - (n1.z - cosA*n2.z)*(cosA*n1.z - n2.z*cosA_2)*rsin2A)*under;

		return theta;
	};

	RealNumber calcAngle(const XYZ& v1, const XYZ& v2, const XYZ& v3, XYZ dx[3], HessianItem dx2[9])
	{
		RealNumber theta = calcAngle(v1-v2, v3-v2, dx, dx2);
		dx[2] = dx[1];
		dx[1] = -dx[1] - dx[0];

		dx2[8] = dx2[3];
		dx2[6] = dx2[2];
		dx2[2] = dx2[1];

		dx2[1] = -(dx2[0] + dx2[2]);
		dx2[3] = -(dx2[0] + dx2[6]);
		dx2[4] = -(dx2[0] + dx2[8]);
		dx2[5] = -(dx2[2] + dx2[8]);
		dx2[7] = -(dx2[6] + dx2[8]);

		return theta;
	};

	RealNumber calcCosAngle(const XYZ& v1, const XYZ& v2, const XYZ& v3,  XYZ dx[3], HessianItem dx2[9]) {
		RealNumber theta = calcCosAngle(v1-v2, v3-v2, dx, dx2);
		dx[2] = dx[1];
		dx[1] = -dx[1] - dx[0];

		dx2[8] = dx2[3];
		dx2[6] = dx2[2];
		dx2[2] = dx2[1];

		dx2[1] = -(dx2[0] + dx2[2]);
		dx2[3] = -(dx2[0] + dx2[6]);
		dx2[4] = -(dx2[0] + dx2[8]);
		dx2[5] = -(dx2[2] + dx2[8]);
		dx2[7] = -(dx2[6] + dx2[8]);

		return theta;
	}

	RealNumber calcCosTorsion(const XYZ& v1, const XYZ& v2, const XYZ& v3, const XYZ& v4) 
	{
        assert (abs(v1-v2) > 0);
		assert (abs(v2-v3) > 0);
		assert (abs(v3-v4) > 0);

		XYZ v12 = v1 - v2;
		XYZ axis = v2 - v3;
		XYZ v43 = v4 - v3;
		return calcCosAngle(v12*axis, v43*axis);
	}


	RealNumber calcTorsion(const XYZ& v1, const XYZ& v2, const XYZ& v3, const XYZ& v4) 
	{
        assert (abs(v1-v2) > 0);
		assert (abs(v2-v3) > 0);
		assert (abs(v3-v4) > 0);

		XYZ v12 = v1 - v2;
		XYZ axis = v2 - v3;
		XYZ v43 = v4 - v3;
		return calcDehedralAngle(v12, v43, axis);
	}

	RealNumber calcCosTorsion(const XYZ& v1, const XYZ& v2, const XYZ& v3, const XYZ& v4, XYZ dx[4]) 
	{
		XYZ temp[3];
		RealNumber result = calcCosTorsion(v1 - v2, v4 - v3, v2 - v3, temp);
		dx[0] = temp[0];
		dx[1] = temp[2]-temp[0];
		dx[2] = -temp[1]-temp[2];
		dx[3] = temp[1];
		return result;
	}

	RealNumber calcCosTorsion(const XYZ& v1, const XYZ& v2, const XYZ& va, XYZ dx[3], HessianItem dx2[9]) {
		XYZ n1 = v1*va, n2 = v2*va;
		RealNumber r1 = abs(n1), r2 = abs(n2);
		if (r1 < 1e-30 || r2 < 1e-30) {
			return 0;
		}
		RealNumber rr1 = 1/r1, rr2 = 1/r2;
		RealNumber rr11 = rr1*rr1, rr22 = rr2*rr2, rr12 = rr1*rr2;

		RealNumber x1_2 = v1.x*v1.x, y1_2 = v1.y*v1.y, z1_2 = v1.z*v1.z;
		RealNumber x2_2 = v2.x*v2.x, y2_2 = v2.y*v2.y, z2_2 = v2.z*v2.z;
		RealNumber xa_2 = va.x*va.x, ya_2 = va.y*va.y, za_2 = va.z*va.z;

		XYZ vn1xva = n1*va, vn1xv1 = n1*v1, vn1xv2 = n1*v2;
		XYZ vn2xva = n2*va, vn2xv1 = n2*v1, vn2xv2 = n2*v2;

		RealNumber dotN = dot(n1, n2);
		RealNumber cost = dotN*rr12;

		dx[0] = vn1xva; dx[0] *= dotN*rr11; dx[0] -= vn2xva; dx[0] *= rr12;
		dx[1] = vn2xva; dx[1] *= dotN*rr22; dx[1] -= vn1xva; dx[1] *= rr12;
		dx[2] = (vn1xv1*rr11 + vn2xv2*rr22); dx[2] *= -dotN;
		dx[2] += vn1xv2; dx[2] += vn2xv1; dx[2] *= rr12;

		HessianItem item1 = diag(dot(v1, v1)) - prod(v1, v1);
		HessianItem item2 = diag(dot(v2, v2)) - prod(v2, v2);
		HessianItem itemA = diag(dot(va, va)) - prod(va, va);

		RealNumber v1A[] = {
			v1.y*va.y +   va.z*v1.z, v1.y*va.x - 2*v1.x*va.y, va.x*v1.z - 2*v1.x*va.z,
			v1.x*va.y - 2*va.x*v1.y, v1.x*va.x +   v1.z*va.z, va.y*v1.z - 2*v1.y*va.z,
			v1.x*va.z - 2*va.x*v1.z, v1.y*va.z - 2*v1.z*va.y, va.x*v1.x +   v1.y*va.y
		};
		RealNumber v2A[] = {
			v2.y*va.y +   va.z*v2.z, v2.y*va.x - 2*v2.x*va.y, va.x*v2.z - 2*v2.x*va.z,
			v2.x*va.y - 2*va.x*v2.y, v2.x*va.x +   v2.z*va.z, va.y*v2.z - 2*v2.y*va.z,
			v2.x*va.z - 2*va.x*v2.z, v2.y*va.z - 2*v2.z*va.y, va.x*v2.x +   v2.y*va.y
		};


		HessianItem vn1xvaXvn1xva=prod(vn1xva),			vn2xvaXvn2xva=prod(vn2xva);
		HessianItem vn2xvaXvn1xva=prod(vn2xva, vn1xva), vn1xvaXvn2xva=prod(vn1xva, vn2xva);
		HessianItem vn1xv1Xvn2xva=prod(vn1xv1, vn2xva), vn2xv2Xvn2xva=prod(vn2xv2, vn2xva);
		HessianItem vn1xvaXvn1xv1=prod(vn1xva, vn1xv1), vn1xvaXvn2xv2=prod(vn1xva, vn2xv2);
		HessianItem vn1xv1Xvn1xva=prod(vn1xv1, vn1xva), vn2xvaXvn2xv2=prod(vn2xva, vn2xv2);
		HessianItem vn2xvaXvn1xv1=prod(vn2xva, vn1xv1); 

		dx2[0] = rr11*rr12*(dotN*(3*rr11*vn1xvaXvn1xva - itemA)- vn1xvaXvn2xva - vn2xvaXvn1xva);
		dx2[1] =      rr12*(itemA - rr11*vn1xvaXvn1xva - rr22*vn2xvaXvn2xva + dotN*rr11*rr22*vn1xvaXvn2xva);
		dx2[2] =      rr12*(rr22*vn2xvaXvn2xv2-HessianItem(v2A) +
							rr11*(prod(vn1xva, vn1xv2 + vn2xv1) + vn2xvaXvn1xv1 +
							      dotN*(HessianItem(v1A) - 3*rr11*vn1xvaXvn1xv1 - rr22*vn1xvaXvn2xv2)));
		dx2[3] = trans(dx2[1]);
		dx2[4] = rr22*rr12*(dotN*(3*rr22*vn2xvaXvn2xva - itemA)- vn1xvaXvn2xva - vn2xvaXvn1xva);

		dx2[5] =      rr12*(rr11*vn1xvaXvn1xv1-HessianItem(v1A) + 
			                rr22*(prod(vn2xva, vn1xv2 + vn2xv1) + vn1xvaXvn2xv2 +
							      dotN*(HessianItem(v2A) - 3*rr22*vn2xvaXvn2xv2 - rr11*vn2xvaXvn1xv1)));

		dx2[6] = trans(dx2[2]);
		dx2[7] = trans(dx2[5]);

		RealNumber v12[] = {
			2*(v1.y*v2.y + v1.z*v2.z),  -v1.x*v2.y - v1.y*v2.x,   -v1.x*v2.z - v1.z*v2.x,
			  -v1.x*v2.y - v1.y*v2.x, 2*(v1.x*v2.x + v1.z*v2.z),  -v1.y*v2.z - v1.z*v2.y,
			  -v1.x*v2.z - v1.z*v2.x,   -v1.y*v2.z - v1.z*v2.y, 2*(v1.x*v2.x + v1.y*v2.y)
		};

		dx2[8] = rr12*(HessianItem(v12) - rr11*(prod(vn1xv1, vn1xv2+vn2xv1) + prod(vn1xv2+vn2xv1, vn1xv1)) - rr22*(prod(vn2xv2, vn1xv2+vn2xv1)+prod(vn1xv2+vn2xv1, vn2xv2)) +
		               dotN*(3*rr11*rr11*prod(vn1xv1) + 3*rr22*rr22*prod(vn2xv2) - item1*rr11 -item2*rr22 + rr11*rr22*(prod(vn1xv1, vn2xv2) + prod(vn2xv2,vn1xv1))));

		return cost;
	}

	RealNumber calcTorsion(const XYZ& v1, const XYZ& v2, const XYZ& v3, const XYZ& v4, XYZ dx[4]) 
	{
		XYZ temp[3];
		RealNumber result = calc1stDerivationOfDehedralAngle(v1 - v2, v4 - v3, v2 - v3, temp);
		dx[0] = temp[0];
		dx[1] = temp[2]-temp[0];
		dx[2] = -temp[1]-temp[2];
		dx[3] = temp[1];
		return result;
	}
	
	RealNumber calcTorsion(const XYZ& v1, const XYZ& v2, const XYZ& vaxis, XYZ dx[3]) 
	{
		return calc1stDerivationOfDehedralAngle(v1, v2, vaxis, dx);
	}
	

}
